#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import LaserScan
from geometry_msgs.msg import Twist
import math
import numpy as np
from collections import deque

class LaserFollower(Node):
    def __init__(self):
        super().__init__('laser_follower')
        
        # 参数设置
        self.declare_parameter('follow_distance', 0.4)      # 保持的跟随距离(米)
        self.declare_parameter('max_linear_speed', 1.0)     # 最大线速度(m/s)
        self.declare_parameter('max_angular_speed', 1.5)    # 最大角速度(rad/s)
        self.declare_parameter('obstacle_threshold', 0.3)   # 障碍物距离阈值(米)
        
        # 角度范围设置为-30度到30度（转换为弧度）
        self.angle_min = -30 * math.pi / 180
        self.angle_max = 30 * math.pi / 180
        
        self.follow_distance = self.get_parameter('follow_distance').value
        self.max_linear_speed = self.get_parameter('max_linear_speed').value
        self.max_angular_speed = self.get_parameter('max_angular_speed').value
        self.obstacle_threshold = self.get_parameter('obstacle_threshold').value
        
        # 订阅激光雷达数据
        self.scan_sub = self.create_subscription(
            LaserScan,
            '/scan',
            self.scan_callback,
            10)
        
        # 发布控制指令
        self.cmd_vel_pub = self.create_publisher(Twist, '/cmd_vel', 10)
        
        # 20Hz控制定时器
        self.timer = self.create_timer(0.05, self.control_loop)
        
        # 历史数据缓存（用于平滑）
        self.distance_history = deque(maxlen=3)
        self.angle_history = deque(maxlen=3)
        self.object_detected = False
        
        self.get_logger().info("激光跟随节点已启动，检测角度范围: -30° ~ 30°")

    def scan_callback(self, msg):
        # 转换为numpy数组提高处理速度
        angles = np.arange(msg.angle_min, msg.angle_max, msg.angle_increment)
        ranges = np.array(msg.ranges)
        
        # 创建有效数据掩码
        valid_mask = (angles >= self.angle_min) & \
                    (angles <= self.angle_max) & \
                    (ranges >= msg.range_min) & \
                    (ranges <= msg.range_max) & \
                    (ranges > self.obstacle_threshold)
        
        valid_ranges = ranges[valid_mask]
        valid_angles = angles[valid_mask]
        
        if len(valid_ranges) > 0:
            min_idx = np.argmin(valid_ranges)
            self.distance_history.append(valid_ranges[min_idx])
            self.angle_history.append(valid_angles[min_idx])
            self.object_detected = True
        else:
            self.object_detected = False

    def control_loop(self):
        cmd = Twist()
        
        if self.object_detected and len(self.distance_history) > 0:
            # 使用平滑后的数据
            current_distance = np.mean(self.distance_history)
            current_angle = np.mean(self.angle_history)
            
            # 计算距离误差（关键修改：确保能后退）
            distance_error = current_distance - self.follow_distance
            
            # 动态调整控制增益（后退时更敏感）
            if distance_error > 0:  # 需要前进
                linear_speed = 0.6 * distance_error
            else:  # 需要后退
                linear_speed = 1.2 * distance_error  # 后退增益更大
            
            # 限制速度范围
            linear_speed = np.clip(
                linear_speed,
                -self.max_linear_speed,
                self.max_linear_speed
            )
            
            # 角度控制（P控制）
            angular_speed = 1.5 * current_angle
            angular_speed = np.clip(
                angular_speed,
                -self.max_angular_speed,
                self.max_angular_speed
            )
            
            cmd.linear.x = linear_speed
            cmd.angular.z = angular_speed
            
            self.get_logger().info(
                f"跟随中 距离: {current_distance:.2f}m | "
                f"速度: {cmd.linear.x:.2f}m/s ({'前进' if cmd.linear.x >0 else '后退'}) | "
                f"转向: {math.degrees(cmd.angular.z):.1f}°"
            )
        else:
            cmd.linear.x = 0.0
            cmd.angular.z = 0.0
            self.get_logger().info("未检测到有效目标，已停止", throttle_duration_sec=1)
        
        self.cmd_vel_pub.publish(cmd)

def main(args=None):
    rclpy.init(args=args)
    node = LaserFollower()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        node.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()